﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Text;
using Microsoft.Ccr.Core;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using Microsoft.Robotics.Services.IRobot.Lite.Proxy;
using Microsoft.Robotics.Services.IRobot.Roomba.Proxy;
using Microsoft.Robotics.Services.IRobot.SensorUpdates.Proxy;
using NestedParticleFilter;
using create = Microsoft.Robotics.Services.IRobot.Create.Proxy;
using lite = Microsoft.Robotics.Services.IRobot.Lite.Proxy;
namespace MatLocalization
{
    partial class MatLocalizationService : DsspServiceBase
    {
        private const int StopVelocity = 0;
        //public static int ForwardVelocity = 200; //mm/second
        private const double DistanceBtweenWheels = 254; //millimeters mm
        private const double StraightOmega = 0.0005;


        /// <summary>
        /// Drive the robot with the specified velocity and rotational velocity
        /// </summary>
        /// <param name="velocity">velocity in mm/sec </param>
        /// <param name="omega">rotational velocity in degree/sec</param>
        /// <param name="drivePort">drive port of the robot</param>
        private static void InternalDrive(double velocity, double omega, IRobotLiteOperations drivePort)
        {
            omega = UnitConversion.DegreeToRadian(omega);  // converts rotational velocity to radian per sec.
            // calculate the velocity of the left and right wheels based on omega and velocity
            int vLeft = (int)((2 * velocity + DistanceBtweenWheels * omega) / 2);
            int vRight = (int)((2 * velocity - DistanceBtweenWheels * omega) / 2);
            Console.WriteLine("Write and left wheel velocity: " + vRight + " " + vLeft);
            drivePort.CreateDriveDirect(vRight, vLeft);// send the request to drive service 
        }

        /// <summary>
        /// Stops the robot immidiately
        /// </summary>
        private static void InternalStop(IRobotLiteOperations drivePort)
        {
            drivePort.CreateDriveDirect(StopVelocity, StopVelocity);
        }

        /// <summary>
        /// Travel Straigth with the given velocity (mm/sec) for left and right wheel.
        /// No propagation is done since the end time is not indicated. 
        /// </summary>
        /// <param name="leftWheel">velocity of the left wheel (mm/sec)</param>
        /// <param name="rightWheel">velocity of the right wheel (mm/sec)</param>
        /// <param name="drivePort">drive port of the robot</param>
        /// <param name="queue"></param>
        /// <returns>ITask for dispatcher to execute</returns>
        public IEnumerator<ITask> GoStraight(int leftWheel, int rightWheel, lite.IRobotLiteOperations drivePort, bool queue)
        {
            if (queue)
            {
                drivePort.CreateDriveDirect(rightWheel, leftWheel);
                _state.StartDriveTime = DateTime.UtcNow;
            }
            else
                drivePort.CreateDriveDirect(rightWheel, leftWheel);

            yield break;

        }



        /// <summary>
        /// Travel straight for certain time with the given velocity. If the velocity is negative,
        /// the robot will travel backwards. Posts a true message in _completed port indicating the action is complete
        /// Queueing the action is done inside this method since the actions and time is given
        /// </summary>
        /// <param name="velocity">robot's velocity in mm/s </param>
        /// <param name="time">time to execute the command in ms </param>
        /// <param name="drivePort"></param>
        /// <returns>ITask for dispatcher to execute</returns>
        public IEnumerator<ITask> GoStraight(int velocity, double time, IRobotLiteOperations drivePort, Port<bool> completed, bool queue)
        {
            if (queue)
            {
                _state.StartDriveTime = DateTime.UtcNow;
                InternalDrive(velocity, StraightOmega, drivePort); // travel straight
                yield return TimeoutPort((int)time).Receive();
                InternalStop(drivePort); // stop the robot 
                _state.StopDriveTime = DateTime.UtcNow;
                _state.PrevLeftWheelVelocity = velocity;
                _state.PrevRightWheelVelocity = velocity;
                yield return new IterativeTask(() => QueueActions(time / 1000)); // queue the actions for sampling
            }
            else
            {
                InternalDrive(velocity, StraightOmega, drivePort); // travel straight
                yield return TimeoutPort((int)time).Receive();
                InternalStop(drivePort); // stop the robot 
            }
            completed.Post(true);
        }
        /// <summary>
        /// Travel Straigth with the given velocity (mm/sec) for left and right wheel.
        /// No propagation is done since the end time is not indicated. 
        /// </summary>
        /// <param name="leftWheel">velocity of the left wheel (mm/sec)</param>
        /// <param name="rightWheel">velocity of the right wheel (mm/sec)</param>
        /// <param name="time">the time the robot moves with the specified left and right wheel velocity</param>
        /// <param name="drivePort">drive port of the robot</param>
        /// <param name="completed">Port to signal an action is completed</param>
        /// <param name="queue"></param>
        /// <returns>ITask for dispatcher to execute</returns>
        public IEnumerator<ITask> MoveForward(int leftWheel, int rightWheel, double time, lite.IRobotLiteOperations drivePort, Port<bool> completed, bool queue)
        {
            if (queue)
            {
                drivePort.CreateDriveDirect(rightWheel, leftWheel);
                _state.StartDriveTime = DateTime.UtcNow;
                yield return TimeoutPort((int)time).Receive();
                InternalStop(drivePort); // stop the robot 
                _state.StopDriveTime = DateTime.UtcNow;
                _state.PrevLeftWheelVelocity = leftWheel;
                _state.PrevRightWheelVelocity = rightWheel;
                yield return new IterativeTask(() => QueueActions(time / 1000)); // queue the actions for sampling
            }
            else
            {
                drivePort.CreateDriveDirect(rightWheel, leftWheel);
                yield return TimeoutPort((int)time).Receive();
                InternalStop(drivePort); // stop the robot 
            }
            completed.Post(true);
            yield break;

        }

        /// <summary>
        /// Rotate in Place. positive degree is counter-clockwise and negative degree is 
        /// clockwise. Posts a true message in completed Port indicating the action is complete
        /// </summary>
        /// <param name="angle">Angle to rotate in degree</param>
        /// <param name="velocity">velcotiy of the robot mm/sec</param>
        /// <param name="drivePort">drive port of the robot</param>
        /// <param name="completed"> complete port to signal the completion of the action</param>
        /// <param name="queue"></param>
        /// <returns></returns>
        public IEnumerator<ITask> RotateRight(double angle, int velocity, IRobotLiteOperations drivePort, Port<bool> completed, bool queue)
        {
            // time = ω/θ 
            double omega = Omega(-velocity, velocity); // in degree/second 
            double time = Math.Abs(angle) / Math.Abs(omega); // second
            if (queue)
            {
                _state.StartDriveTime = DateTime.UtcNow;
                drivePort.CreateDriveDirect(-velocity, velocity);
                yield return TimeoutPort((int)(time * 1000)).Receive();
                InternalStop(drivePort);
                _state.StopDriveTime = DateTime.UtcNow;
                _state.PrevLeftWheelVelocity = velocity;
                _state.PrevRightWheelVelocity = -velocity;
                yield return new IterativeTask(() => QueueActions(time));
            }
            else
            {
                drivePort.CreateDriveDirect(-velocity, velocity);
                yield return TimeoutPort((int)(time * 1000)).Receive();
                Debug.WriteLine("Rotate right time with speed " + velocity + " is: " + time * 1000 + " ms.");

                InternalStop(drivePort);
            }
            completed.Post(true);
        }

        /// <summary>
        /// Rotate in Place. positive degree is counter clockwise and negative degree is 
        /// clockwise. Posts a true message in _driveCompletionPort indicating the action is complete
        /// </summary>
        /// <param name="angle">Angle to rotate in degree</param>
        /// <param name="velocity">robot's velocity in mm/s</param>
        /// <param name="drivePort"></param>
        /// <param name="completed"></param>
        /// <returns></returns>
        public IEnumerator<ITask> RotateLeft(double angle, int velocity, lite.IRobotLiteOperations drivePort, Port<bool> completed, bool queue)
        {
            // t = ω/θ 
            double omega = Omega(velocity, -velocity); // in degree/second
            double time = Math.Abs(angle) / Math.Abs(omega); // in second
            if (queue)
            {
                _state.StartDriveTime = DateTime.UtcNow;
                drivePort.CreateDriveDirect(velocity, -velocity);
                yield return TimeoutPort((int)(time * 1000)).Receive();
                InternalStop(drivePort);
                _state.StopDriveTime = DateTime.UtcNow;
                _state.PrevLeftWheelVelocity = -velocity;
                _state.PrevRightWheelVelocity = velocity;
                yield return new IterativeTask(() => QueueActions(time));
            }
            else
            {
                drivePort.CreateDriveDirect(velocity, -velocity);
                yield return TimeoutPort((int)(time * 1000)).Receive();
                InternalStop(drivePort);
            }
            completed.Post(true);
        }
        /// <summary>
        /// Stops the robot immidately 
        /// </summary>
        /// <returns></returns>
        public IEnumerator<ITask> StopRobot(IRobotLiteOperations drivePort, Port<bool> completed)
        {
            drivePort.CreateDriveDirect(StopVelocity, StopVelocity);
            completed.Post(true);
            yield break;

        }
        /// <summary>
        /// Returns the rotational velocity based on the left and right wheel in degree/second
        /// </summary>
        /// <param name="leftVelocity">left velocity mm/second</param>
        /// <param name="rightVelocity">right velocity mm/second</param>
        /// <returns>rotational velocity (degree/second) </returns>
        public double Omega(double rightVelocity, double leftVelocity)
        {
            // ω = (vl-vr)/distance between wheels
            if (leftVelocity == rightVelocity)
                return StraightOmega;
            double o = (leftVelocity - rightVelocity) / DistanceBtweenWheels;
            o = UnitConversion.RadianToDegree(o);
            return (o);
        }

        /// <summary>
        /// Velocity of the robot based on the left and right wheel speed. (mm/sec)
        /// </summary>
        /// <param name="leftWheel">left wheel velocity (mm/sec)</param>
        /// <param name="rightWheel">right wheel velocity (mm/sec)</param>
        /// <returns>the velocity of the robot (mm/sec) </returns>
        public double Velocity(double leftWheel, double rightWheel)
        {
            if (leftWheel == -rightWheel)
                return 0.0001; // rotting in place
            // return leftWheel;
            return ((leftWheel + rightWheel) / 2.0);
        }

        /// <summary>
        /// Returns the time elapsed between the startTime and endTime in seconds
        /// </summary>
        /// <param name="startTime">start time</param>
        /// <param name="endTime">end time</param>
        /// <returns>total time elapsed in seconds.</returns>
        private double TimeElapsedInSeconds(DateTime startTime, DateTime endTime)
        {
            TimeSpan timeSpan = endTime - startTime;
            Debug.Assert(DateTime.Compare(startTime, endTime) < 0, " The endtime " + endTime.ToString() + " should be later than the start time" + startTime.ToString());

            return (timeSpan.TotalMilliseconds) / 1000.0;
        }
    }
}
